// **********************************************************
//  *
//  *  Copyright (c) 2003  SeikoEpson Inc
//  *  All Rights Reserved
//  *
//  *  File name : f_COS.S
//  *  Function  :
//  *      C Standard Library Function "cos"
//  *      The "cos" function calculates the cosine of x which is radian.
//  *      This file is copied from math.lib of 
//                     CC33 tool(CC33v40.exe + CCPA45.exe, 
//                     math of ansilib33v25 )
//  *
//  *  original file's Revision  :
//  *      1999/12/08  Created by H.Ishida
//  *  Revision  :
//  *      2003/04/04  IrumaSoft M.Takeishi   start
//  *      2003/06/24  IrumaSoft M.Takeishi   changed for gnu33 and 
//  *                                          got more paformance
//  *
//  **********************************************************/
// 
//  --- Format of floating point (double precision) ---
// 
//   31|30         23|22                0
//    -----------------------------------
//   | |  exponent   |   fraction        |
//    -----------------------------------
//   |    8 bits     |    23 bits        |


#include	"f_mathf.def"

// ==============================================================================
// 	Function: double cos(double x)
// 
// 	Input:  %r6: Argument x in radian float.
// 
// 	Output: %r4: Return value, cosine of x in float.
// 
	.text
	.align	1
	.global	f_cos

f_cos:

	//  NaN or INF check
	ld.w	%r7,%r6
	xld.w	%r8, 0x7f800000
	and	%r7, %r8
	cmp	%r7, %r8
	xjrne	_GoodArgument
	//  set errno and NaN
	xld.w	%r4, 0x7fc00000
	
	xld.w	%r7,EDOM
	xld.w	[errno],%r7		//  data pointer is needed
	ret

_GoodArgument:
	pushn	%r3			//  Save registers %r3...%r0.
	sub	%sp,1			//  Presearve working area in five words.

	// (delete)[%sp+12] = Taylor series's sign parameter (low).
	//  %r3     = Taylor series's sign parameter.
	//  [%sp+16] 
	//  %r2     = argument x.

	//  %r1     = Quadrant of trigonometric function (word).
	// [%sp+08]

	// [%sp+00] = argument of modf, pointer to its fraction part (float).

	ld.w	%r2,%r6			//  argument x.
// 
// 	Clear the sign bit of argument x in %r2 by shifting
// 	to make it a plus value.
// 
	sll	%r2,1			//  Shift the argument x to the left  one bit.
	srl	%r2,1			//  Shift the argument x to the right one bit.
 
	ld.w	%r1,0x1			//  Initialize the quadrant to 1.

// ##### REMOVED 12/08/1999 #############
// # 	sra		%r7,1		//  Check if x is minus?
// # 	xjrge	_ArgIsNotMinus		//  Branch if plus or zero.
// # // 
// # // 	if the argument x is minus...
// # // 
// # 	xld.w	%r10,0x3		//  Set the quadrant 3.
// # 	xld.w	[%sp+8],%r10		//  Set the quadrant 3.
// ##### REMOVE END #####################
// 
// 	Mutiply the argument x by the constant 2*Pai
// 
_ArgIsNotMinus:
	ld.w	%r6,%r2			//  Argument x.
	xld.w	%r7,F_TWO_PAI		//  Multiplier, constant 2*Pai.
					//  
	xcall	__mulsf3		//  Calculate multiplication//  argument x = x * (2 * Pai).
					//  The result product is %r4.
	ld.w	%r2,%r4			//  Put result x in %r2.
// 
// 	Check if the argument x is large radian.
// 	If large, do some argument reduction, else skip reduction procedure.
// 
	xld.w	%r0,LARGE_RAD		//  Set the large radian constant.
	ld.w	%r6,%r2			//  Argument x.
	ld.w	%r7,%r0			//  Large radian constant.
					//  Compare argument x and the large radian.
	xcall	__fcmps			//  to check if the argument x is large.
	xjrle	_ArgIsNotLarge		//  Skip if the argument x is not large.
// 
// 	--- Argument x is large ---
// 	Reduce the argument to avoid overflow.
// 
	ld.w	%r6,%r2			//  Argument x (low  word), 1st parameter of f_modf().
	ld.w	%r7,%sp			//  Pointer to the fraction part [%sp+00], the 2nd parameter of f_modf().
					//  
	xcall	f_modf			//  Calculate the integer part and the fraction part.
// 
// 	Use the integer part as a Tayler series's sign parameter.
// 
	ld.w	%r3,%r4			//  Tayler series's sign parameter.
// 
// 	Use the fraction part as a new argument x.
// 
	xld.w	%r2,[%sp]		//  Copy fraction result to the argument x.

// 
// 	Adjust the new argument 
// 	by adding the quadrant.
// 
	ld.w	%r6,%r1			//  Load the quadrant integer.
					//  
	xcall	__floatsisf		//  Convert the quadrant to a float value.
					// 
	ld.w	%r6,%r2			//  Argument x.
	ld.w	%r7,%r4			//  Quadrant in float.
					// 
	xcall	__addsf3		//  Add the argument x and the quadrant.
					//  The result is %r4.
	ld.w	%r2,%r4			//  Put the result back to the argument x.

// # 		modf( 0.25 * x, &f )// 

	ld.w	%r6,%r2			//  Argument x.
	xld.w	%r7,QUARTER		//  Constant 0.25 in float.
					// 
	xcall	__mulsf3		//  Calculate multiplication x*0.25.
								// 
	ld.w	%r6,%r4			//  Set x*0.25 result as 1st param of f_modf.
	ld.w	%r7,%sp			//  2nd param of f_modf, pointer to the fraction.
					// 
	xcall	f_modf			//  call f_MODF, another library function.
// 
// 	Calculate the new quadrant
// 	by subtracting 4.0 from the argument x
// 	and then multiplying by the fraction part of the argument.
// 	(argument x - 4.0 * fraction)
// 
	xld.w	%r6,[%sp]		//  Load the fraction part.
	xld.w	%r7,FOUR		//  Set a constant 4.0.
					//  
	xcall	__mulsf3		//  Caculate fraction*4.0.
					//  
	ld.w	%r6,%r2			//  Set the argument x.
	ld.w	%r7,%r4			//  Set the result of fraction*4.0.
					//  
	xcall	__subsf3		//  Calculate fraction*4.0-x.
					//  
	ld.w	%r6,%r4			//  Put the result back to the argument x (low).
					//  
	xcall	__fixsfsi		//  Make it into an integer (resulting quadrant).
					//  

	ld.w	%r1,%r4			//  Store the new quadrant.
	cmp	%r1,0
	xjp	_ChkIfQuadrant1or3	//  Jump to the next process,
					//  completing the adjustment, 
					//  when the argument x is large.
// 
// 	--- Argument x is not large ---
// 
// 	Obtain the integer part of the argument x in double
// 	to use it for adjusting the Taylor series's sign parameter
// 	accoring to the quadrant.
// 
_ArgIsNotLarge:
	ld.w	%r6,%r2			//  Argument x (low  word).
					//  
	xcall	__fixsfsi		//  Cast the argument x in float to an integer.
					//  %r4 <- Result integer.
	ld.w	%r0,%r4			//  Keep the integer part of the argument x in %r0.
// 
// 	Obtain the fraction part of the argument x 
// 	by subtracting the integer part.
// 
	ld.w	%r6,%r0			//  Set the integer part value as input to "__floatsisf".
					//  
	xcall	__floatsisf		//  Cast the integer part back to a float value.
					//  
	ld.w	%r6,%r2			//  Set the resulting float as subtrahend of __subsf3.
	ld.w	%r7,%r4			//  Set the result of former __floatsisf as the next subtracter.
					//  
	xcall	__subsf3		//  Subtract %13 (argument x) from %r6 (integer argument).
					//  
	ld.w	%r3,%r4			//  Set the subtraction result back to the argument x.
// 
// 	Calculate the new quadtrant
// 	by adding the integer part of the argument x
// 	and then logically anding with a constant 3.
// 
	add	%r1,%r0			//  Adding integer part and the argument x.
	and	%r1,0x3			//  Anding it with a consant 3.
// 
// 	--- Checkng according to the quadrant ---
// 	Check if the quadrant is 1 or 3 (odd number).
// 
_ChkIfQuadrant1or3:
	ld.w	%r4,%r1			//  Load the quadrant
	xand	%r4,0x1			//  Check if the quadrant is 1 or 3?
	xjreq	_ChkIfQuadMoreThan1	//  Skip if the quadrant is neither 1 nor 3.
// 
// 	--- Quadrant is 1 or 3 ---
// 	Adjust the Taylor series's sign parameter
// 	by calculating "param = 1 - param".
// 
	xld.w	%r6,ONE			//  Set constant 1.0.
	ld.w	%r7,%r3			//  Taylor series's sign parameter.
					//  
	xcall	__subsf3		//  Subtract %r6 - %r7, Result is %r4
					//  
	ld.w	%r3,%r4			//  Store the result of calculation.
// 
// 	--- Checking if the quadrant is larger than 1 ---
// 
_ChkIfQuadMoreThan1:
// 					//  Loading the quadrant.
	xcmp	%r1,1			//  Check if it is larger than 1?
	xjrle	_SquareTaylorParam	//  If not larger than 1, skip to complete calculating Tayler's param.
// 
// 	--- Quadrant is larget than 1 ---
// 	then, negate the Taylor series's sign parameter.
// 
	ld.w	%r7,%r3			//  Load the Tayler series's parameter.
	XXOR	%r7,SIGN_MASK		//  Negate it.
	ld.w	%r3,%r7			//  Store it.
// 	
// 	--- Calculate the square of the Tayler series's sign parameter ---
// 	Complete adjusting the Taylor series's sign parameter
// 	by calculating its square.
// 
_SquareTaylorParam:
	ld.w	%r6,%r3			//  Multiplicand, the argument x.
	ld.w	%r7,%r3			//  Multiplier, the argument x.
					//  
	xcall	__mulsf3		//  Calculate square, the argument x * argument x
					//  
	ld.w	%r2,%r4			//  Put the square result in the argument x.
// 
// 	
// 
// 	(((((P4 * x + P3) * x + P2) * x + P1) * x + P0) * y)
// 	----------------------------------------------------
// 	           (((x + Q2) * x + Q1) * x + Q0)
// 
	ld.w	%r6,%r2			//  Argument square x.
	xld.w	%r7,CP4			//  float 2.80782741762206855540e0.
					//  
	xcall	__mulsf3		//  
					//  
	ld.w	%r6,%r4			//  
	xld.w	%r7,CP3			//  float -2.37859324578121572813e2.
					//  
	xcall	__addsf3		//  
					//  
	ld.w	%r6,%r4			//  
	ld.w	%r7,%r2			//  
					//  
	xcall	__mulsf3		//  
					//  
	ld.w	%r6,%r4			//  
	xld.w	%r7,CP2			//  float 7.06413608140068845387e3.
					//  
	xcall	__addsf3		//  
					//  
	ld.w	%r6,%r4			//  
	ld.w	%r7,%r2			//  
					//  
	xcall	__mulsf3		//  
					//  
	ld.w	%r6,%r4			//  
	xld.w	%r7,CP1			//  float -7.65864156388469564263e4.
					//  
	xcall	__addsf3		//  
					//  
	ld.w	%r6,%r4			//  
	ld.w	%r7,%r2			//  
					//  
	xcall	__mulsf3		//  
					//  
	ld.w	%r6,%r4			//  
	xld.w	%r7,CP0			//  float 2.07823684169610118261e5.
					//  
	xcall	__addsf3		//  
					//  
	ld.w	%r6,%r4			//  
	ld.w	%r7,%r3			//  
					//  
	xcall	__mulsf3		//  
					//  
	ld.w	%r0,%r4			//   push the child part
	ld.w	%r6,%r2			//  
	xld.w	%r7,CQ2			//  float 1.08999811037129049396e2.
					//  
	xcall	__addsf3		//  
					//  
	ld.w	%r6,%r4			//  
	ld.w	%r7,%r2			//  
					//  
	xcall	__mulsf3		//  
					//  
	ld.w	%r6,%r4			//  
	xld.w	%r7,CQ1			//  float 5.65168679531691759621e3.
					//  
	xcall	__addsf3		//  
					//  
	ld.w	%r6,%r4			//  
	ld.w	%r7,%r2			//  
					//  
	xcall	__mulsf3		//  
					//  
	ld.w	%r6,%r4			//  
	xld.w	%r7,CQ0			//  float 1.32304666508649301250e5.
					//  
	xcall	__addsf3		//  
					//  
	ld.w	%r6,%r0			//  pop the child part
	ld.w	%r7,%r4			//  
					//  
	xcall	__divsf3		//  

	add	%sp,1			//  Roll up the working area.
	popn	%r3			//  Restore registers %r3...%r0.
	ret				//  

